#pragma once
#include "tools.h"

bool readIntFromSD(File file, int &intVar) {
  if (file.available()) {
    String line = "";
    char ch;
    while (file.available()) {
      ch = file.read();
      if (ch == ' ' || ch == '\n') break;
      line += ch;
    }
    intVar = line.toInt();
    return true;
  }
  return false;
}

bool readDoubleFromSD(File file, double &doubleVar) {
  if (file.available()) {
    String line = "";
    char ch;
    while (file.available()) {
      ch = file.read();
      if (ch == ' ' || ch == '\n') break;
      line += ch;
    }
    doubleVar = line.toDouble();
    return true;
  }
  return false;
}

bool readWordFromSD(File file, int maxLength, char *cmd) {
  if (file.available()) {
    int index = 0;
    while (file.available() && index < maxLength - 1) {
      char ch = file.read();
      if (index == 0 && (ch == ' ' || ch == '\r' || ch == '\n')) {
        continue;
      }
      if (ch == ' ' || ch == '\n' || ch == '\r') {
        // 如果到达行尾，停止读取
        break;
      }
      cmd[index++] = ch;
    }
    cmd[index] = '\0'; // 确保字符串以空字符结尾
    return true;
  }
  return false;
}

bool readLineFromSD(File file, int maxLength, char *cmd) {
  if (file.available()) {
    int index = 0;
    while (file.available() && index < maxLength - 1) {
      char ch = file.read();
      if (ch == '\n') {
        // 如果到达行尾，停止读取
        cmd[index++] = '\n';
        break;
      }
      cmd[index++] = ch;
    }
    cmd[index] = '\0'; // 确保字符串以空字符结尾
    return true;
  }
  return false;
}

// 将计算特征写入文件
void writeToTxt(const char* filepath, int label, const std::vector<float>& character) {
  // 检查文件是否存在
  // if (!SD.exists(filepath)) {
  //   // 文件不存在，创建一个新文件
  //   File dataFile = SD.open(filepath, FILE_WRITE);
  //   if (!dataFile) {
  //     Serial.println("Failed to create file.");
  //     return;
  //   } else {
  //     Serial.println("Create file success.");
  //   }
  //   dataFile.close(); // 关闭文件
  // } else {
  //   Serial.println("File exist.");
  //   SD.remove(filename);
  //   Serial.print("remove file "); Serial.println(filename); 
  // }
  if (SD.exists(filepath)){
    Serial.print("已存在 "); Serial.println(filepath); 
    SD.remove(filepath);
    Serial.print("remove file "); Serial.println(filepath); 
  }
  if (SD.exists(filepath)){
    Serial.print("已存在 "); Serial.println(filepath); 
  }
  // 以覆盖模式打开文件
  File dataFile;
  dataFile = SD.open(filepath, FILE_WRITE);
  // audioFile = SD.open(filename, FILE_WRITE);

  Serial.print("File name:");
  Serial.println(filepath);
  if (!dataFile) {
    Serial.println("Failed to open file.");
    return;
  } else {
    Serial.println("Open file success.");
  }

  // 将文件指针移到文件开始处
  dataFile.seek(0);

  dataFile.print(label);
  dataFile.print(" ");
  int character_size = character.size();
  for (int i = 0; i < character_size; i++) {
    dataFile.print(i);
    dataFile.print(":");
    dataFile.print(character[i]);
    if (i != (character_size - 1)) {
      dataFile.print(" ");
    }
  }
  dataFile.println(); // 换行

  // 关闭文件
  dataFile.close();
  Serial.println("Write data to file success!");
}


// 实现简单的汉宁窗函数
std::vector<float> hanningWindow(int size) {
	std::vector<float> window(size);
	for (int i = 0; i < size; ++i) {
		window[i] = 0.5f * (1.f - std::cos(2.f * M_PI * i / (size - 1)));
	}
	return window;
}

// 实现简化的STFT函数
std::vector<std::vector<std::vector<float>>> simplifiedSTFT(const std::vector<float>& x, int n_fft, int hop_length, const std::string& window, bool center, const std::string& mode) {
    std::vector<float> win;
    if (window == "hann") {
        win = hanningWindow(n_fft);
    }
    else {
        // 如果需要，添加其他类型的窗函数
    }

    // 如果需要，进行填充
    int pad_len = center ? n_fft / 2 : 0;
    std::vector<float> x_padded(x.size() + 2 * pad_len, 0);
    std::copy(x.begin(), x.end(), x_padded.begin() + pad_len);

    int n_frames = 1 + (x_padded.size() - n_fft) / hop_length;
    std::vector<std::vector<std::vector<float>>> X(n_frames, std::vector<std::vector<float>>(n_fft / 2 + 1, std::vector<float>(2, 0)));

    for (int i = 0; i < n_frames; ++i) {
        std::vector<float> x_frame(n_fft);
        for (int j = 0; j < n_fft; ++j) {
            x_frame[j] = win[j] * x_padded[i * hop_length + j];
        }

        // 执行快速傅立叶变换
        for (int k = 0; k < n_fft / 2 + 1; ++k) {
            float real_sum = 0;
            float imag_sum = 0;
            for (int j = 0; j < n_fft; ++j) {
                real_sum += x_frame[j] * std::cos(2 * M_PI * j * k / n_fft);
                imag_sum -= x_frame[j] * std::sin(2 * M_PI * j * k / n_fft);
            }
            X[i][k][0] = real_sum;
            X[i][k][1] = imag_sum;
        }
    }

    return X;
}

std::vector<std::vector<float>> melFilterBank(int n_fft, int n_mel, int sample_rate)
{
	// 计算频率对应的Mel频率
	std::vector<float> mel_frequencies(n_mel + 2);
	float mel_min = 0;
	float mel_max = 2595 * log10(1 + sample_rate / 2.f / 700);
	for (int i = 0; i < n_mel + 2; ++i) {
		mel_frequencies[i] = mel_min + i * (mel_max - mel_min) / (n_mel + 1);
	}

	// 将Mel频率转换为线性频率
	std::vector<float> frequencies(n_mel + 2);
	for (int i = 0; i < n_mel + 2; ++i) {
		frequencies[i] = 700 * (pow(10, mel_frequencies[i] / 2595) - 1);
	}

	// 初始化梅尔滤波器组
	std::vector<std::vector<float>> filters(n_mel, std::vector<float>(n_fft / 2 + 1, 0));

	for (int i = 0; i < n_mel; ++i) {
		float mel_center = mel_frequencies[i + 1];
		float mel_left = mel_frequencies[i];
		float mel_right = mel_frequencies[i + 2];
		float f_center = frequencies[i + 1];
		float f_left = frequencies[i];
		float f_right = frequencies[i + 2];

		for (int j = 0; j <= n_fft / 2; ++j) {
			float freq = sample_rate * j / static_cast<float>(n_fft);
			if (freq >= f_left && freq <= f_right) {
				if (freq <= f_center)
					filters[i][j] = (freq - f_left) / (f_center - f_left);
				else
					filters[i][j] = (f_right - freq) / (f_right - f_center);
			}
		}
	}

	return filters;
}

std::vector<std::vector<float>> melSpectrogram(const std::vector<float>& x, int sample_rate, int n_fft, int n_hop, const std::string& win, bool center, const std::string& mode, float ref_power, int n_mel, int fmin, int fmax) {
    // 计算STFT
    std::vector<float> window = hanningWindow(n_fft);
    int pad_len = center ? n_fft / 2 : 0;
    std::vector<float> x_padded(x.size() + 2 * pad_len, 0);
    std::copy(x.begin(), x.end(), x_padded.begin() + pad_len);
    int n_frames = 1 + (x_padded.size() - n_fft) / n_hop;
    std::vector<std::vector<std::vector<float>>> stft_result(n_frames, std::vector<std::vector<float>>(n_fft / 2 + 1, std::vector<float>(2, 0)));
    for (int i = 0; i < n_frames; ++i) {
        std::vector<float> x_frame(n_fft);
        for (int j = 0; j < n_fft; ++j) {
            x_frame[j] = window[j] * x_padded[i * n_hop + j];
        }
        for (int k = 0; k < n_fft / 2 + 1; ++k) {
            float real_sum = 0;
            float imag_sum = 0;
            for (int j = 0; j < n_fft; ++j) {
                real_sum += x_frame[j] * std::cos(2 * M_PI * j * k / n_fft);
                imag_sum -= x_frame[j] * std::sin(2 * M_PI * j * k / n_fft);
            }
            stft_result[i][k][0] = real_sum;
            stft_result[i][k][1] = imag_sum;
        }
    }

    // 计算梅尔滤波器组
    std::vector<std::vector<float>> filters = melFilterBank(n_fft, n_mel, sample_rate);

    // 只选择感兴趣的频率范围内的数据
    int n_fmin_bin = static_cast<int>(std::floor(fmin * n_fft / sample_rate));
    int n_fmax_bin = static_cast<int>(std::ceil(fmax * n_fft / sample_rate)) + 1;
    std::vector<std::vector<std::vector<float>>> stft_result_selected(n_frames, std::vector<std::vector<float>>(n_fmax_bin - n_fmin_bin, std::vector<float>(2, 0)));
    for (int i = 0; i < n_frames; ++i) {
        for (int k = 0; k < n_fmax_bin - n_fmin_bin; ++k) {
            stft_result_selected[i][k][0] = stft_result[i][n_fmin_bin + k][0];
            stft_result_selected[i][k][1] = stft_result[i][n_fmin_bin + k][1];
        }
    }

    // 计算梅尔频谱图
    std::vector<std::vector<float>> mel_spectrogram(n_frames, std::vector<float>(n_mel, 0));
    for (int i = 0; i < n_frames; ++i) {
        for (int j = 0; j < n_mel; ++j) {
            float sum = 0;
            for (int k = 0; k < stft_result_selected[i].size(); ++k) {
                float magnitude = std::sqrt(stft_result_selected[i][k][0] * stft_result_selected[i][k][0] + stft_result_selected[i][k][1] * stft_result_selected[i][k][1]);
                sum += magnitude * filters[j][k];
            }
            mel_spectrogram[i][j] = sum;
        }
    }

    return mel_spectrogram;
}
// std::vector<std::vector<float>> melSpectrogram(const std::vector<float>& x, int sample_rate, int n_fft, int n_hop, const std::string& win, bool center, const std::string& mode, float ref_power, int n_mel, int fmin, int fmax)
// {
// 	// 计算STFT
// 	std::vector<float> window = hanningWindow(n_fft);
// 	int pad_len = center ? n_fft / 2 : 0;
// 	std::vector<float> x_padded(x.size() + 2 * pad_len, 0);
// 	std::copy(x.begin(), x.end(), x_padded.begin() + pad_len);
// 	int n_frames = 1 + (x_padded.size() - n_fft) / n_hop;
// 	std::vector<std::vector<Complex>> stft_result(n_frames, std::vector<Complex>(n_fft / 2 + 1, 0));
// 	for (int i = 0; i < n_frames; ++i) {
// 		std::vector<float> x_frame(n_fft);
// 		for (int j = 0; j < n_fft; ++j) {
// 			x_frame[j] = window[j] * x_padded[i * n_hop + j];
// 		}
// 		for (int k = 0; k < n_fft / 2 + 1; ++k) {
// 			Complex sum = 0;
// 			for (int j = 0; j < n_fft; ++j) {
// 				sum += Complex(x_frame[j] * std::cos(2 * M_PI * j * k / n_fft),
// 					-x_frame[j] * std::sin(2 * M_PI * j * k / n_fft));
// 			}
// 			stft_result[i][k] = sum;
// 		}
// 	}

// 	// 计算梅尔滤波器组
// 	std::vector<std::vector<float>> filters = melFilterBank(n_fft, n_mel, sample_rate);

// 	// 只选择感兴趣的频率范围内的数据
// 	int n_fmin_bin = static_cast<int>(std::floor(fmin * n_fft / sample_rate));
// 	int n_fmax_bin = static_cast<int>(std::ceil(fmax * n_fft / sample_rate)) + 1;
// 	std::vector<std::vector<Complex>> stft_result_selected(n_frames, std::vector<Complex>(n_fmax_bin - n_fmin_bin, 0));
// 	for (int i = 0; i < n_frames; ++i) {
// 		std::copy(stft_result[i].begin() + n_fmin_bin, stft_result[i].begin() + n_fmax_bin, stft_result_selected[i].begin());
// 	}

// 	// 计算梅尔频谱图
// 	std::vector<std::vector<float>> mel_spectrogram(n_frames, std::vector<float>(n_mel, 0));
// 	for (int i = 0; i < n_frames; ++i) {
// 		for (int j = 0; j < n_mel; ++j) {
// 			float sum = 0;
// 			for (int k = 0; k < stft_result[i].size(); ++k) {
//         float magnitude = std::sqrt(stft_result[i][k].real() * stft_result[i][k].real() + stft_result[i][k].imag() * stft_result[i][k].imag());
// 				sum += magnitude * filters[j][k];
// 			}
// 			mel_spectrogram[i][j] = sum;
// 		}
// 	}

// 	return mel_spectrogram;
// }
